import sensor, image, time, lcd
from machine import Timer, UART
from fpioa_manager import fm
lcd.init(freq=15000000)
lcd.rotation(2)
sensor.reset()                      # Reset and initialize the sensor. It will
                                    # run automatically, call sensor.run(0) to stop
fm.register(7, fm.fpioa.UART1_RX, force = True)             # 配置 7 脚为 UART1_RX 强制注册
fm.register(8, fm.fpioa.UART1_TX, force = True)             # 配置 8 脚为 UART1_TX 强制注册

uart = UART(UART.UART1, 115200, 8, 0, 1)
sensor.set_hmirror(0)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_gain(False,gain_db = 18) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking


sensor.skip_frames(time = 2000)     # Wait for settings take effect.
sensor.run(1)
#以上为系统拍照初始化参数
osd = [0,0]
center_blob = 0
old_center_blob = 0
drop_frame_counter = 0
traced = False
area = (0,0,320,240)
clock = time.clock()                # Create a clock object to track the FPS.
while(True):
    clock.tick()                    # Update the FPS clock.#接收上位机发来的程序分支字符
    img=sensor.snapshot()
    blobs = img.find_blobs([(46, 100, -8, 56, -81, 50)], merge = True,roi = area) #红色阈值以及光斑尺寸下限
    if blobs:
        center_blob = blobs[0]
        for b in blobs:
            if (center_blob.cx()-160)**2 + (center_blob.cy()-120)**2 > (b.cx()-160)**2 + (b.cy()-120)**2:
                center_blob = b
            img.draw_cross(b.cx(), b.cy())

        if old_center_blob == 0:
            old_center_blob = center_blob
            #如果检测到dx^2+dy^2 < 2000距离内的下一个点

        if (center_blob.cx()-160)**2 + (center_blob.cy()-120)**2 > 500 and not traced :#如果中心点还没追踪靠近过目标点
            old_center_blob = center_blob
        elif (old_center_blob.cx() - center_blob.cx())**2 + (old_center_blob.cy() - center_blob.cy())**2 < 2000:
            #area = (old_center_blob.cx()-40,old_center_blob.cy()-40,80,80)
            old_center_blob = center_blob
            traced = True
            drop_frame_counter = 0 #重置丢帧计数器
        elif drop_frame_counter < 5:
            drop_frame_counter = drop_frame_counter + 1
        else:
            traced = False #丢帧大于n=5帧则认为丢失目标，重新去找。
            drop_frame_counter = 0  #重置丢帧计数器
        #print(old_center_blob)
        #print(center_blob)
        img.draw_cross(old_center_blob.cx(), old_center_blob.cy())
        uart.write("H%03d,%03d" % (old_center_blob.cx(),old_center_blob.cy()))
        osd[0] = old_center_blob.cx()
        osd[1] = old_center_blob.cy()
        img.draw_string(old_center_blob.cx()+5,old_center_blob.cy()+5,"H:[%d,%d]" % (old_center_blob.cx(),old_center_blob.cy()))
    lcd.display(img)                # Display on LCD
    print("%fdb" % sensor.get_gain_db())
    print(clock.fps())              # Note: MaixPy's Cam runs about half as fast when connected
    # to the IDE. The FPS should increase once disconnected.
